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ABSTRACT:  A  new  approach  to  certain  mot ion-planaing  problems  in 
robotics  is  introduced.  This  approach  is  based  on  the  use  of  a 
generalized  Voronoi  diagram,  and  reduces  the  search  for  a 
collision-free  continuous  motion  to  a  search  for  a  connected  path  along 
the  edges  of  such  a  diagram.  This  approach  yields  an  0(n  log  n) 
algorithm  for  planning  an  obstacle-avoiding  motion  of  a  single  circular 
disc  amid  polygonal  obstacles.  Later  papers  will  show  that  extensions 
of  the  approach  can  solve  other  motion-planning  problems,  including 
those  of  moving  a  straight  line-segment  or  two  co-ordinated  discs  in 
the  plane  amid  polygonal  obstacles. 
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0,  Introduction 

The  Piano-Movers'  Problem  has  been  the  subject  of  a  number  of 
recent  papers  [1,7,8,9],  The  2-dimensional  version  of  the  problem 
which  will  concern  us  here  is  to  determine  whether  there  exists  a 
continuous  motion  of  a  given  rigid  body  B  free  to  move  within  a 
2-dimensional  region  fi  bounded  by  a  set  S  of  finitely  many  polygonal 
obstacles,  between  two  specified  placements,  during  which  B  continually 
avoids  collision  with  these  obstacles,  and  to  produce  such  a  motion 
when  one  exists.  The  method  described  in  the  previous  papers  is 
roughly  as  follows:  for  a  given  set  S  of  obstacles  and  a  body  B,  the 
set  FP,  of  all  pairs  [position,  orientation]  which  represent  placements 
of  B  in  which  B  is  not  incident  on  any  obstacle  in  S,  is  an  open 
manifold,  and  a  continuous  obstacle-avoiding  path  between  two  given 
placements  of  B  exists  if  and  only  if  these  placements  belong  to  the 
same  connected  component  of  FP.  Thus  the  problem  is  reduce-^  to  that  of 
finding  the  connected  components  of  FP,  which  is  solved  by  partitioning 
FP  into  a  collection  of  smaller  cells  having  relatively  simple 
structure,  and  by  determining  the  adjacency  relationships  between  these 
cells.  These  relationships  yield  a  finite  'connectivity  graph',  which 
can  then  be  searched  to  determine  the  connected  components  of  FP.  This 
approach  can  be  applied  in  general  situations,  and  has  been  shown  in 
[8,9]  to  yield  polynomial-time  algorithms  for  planning  the  motion  of 
various  bodies  or  groups  of  bodies  B. 

In  this  paper  we  suggest  a  different  approach  to  the  movers' 
problem  and  apply  it  to  the  simple  case  in  which  the  body  B  is  a 
circular  disc.  A  motion-planning  algorithm  of  complexity  0(n  log  n)  is 
shown  for  this  case.  As  suggested  in  [4]  and  used  in  [5],  the  idea  of 
solving  the  motion-planning  problem  for  a  disc  circumscribing  a 
2-dimensional  body  of  more  complex  shape  can  be  used  as  an  initial 
heuristic  in  motion-planning  for  the  body.  Only  if  no  continuous 
motion  exists  for  this  disc  should  more  refined  algorithms  (taking  more 
detail  of  the  body  shape  into  account)  be  brought  to  bear. 
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Throughout  this  paper  we  assume  Chat  the  collection  S  of  obstacles 
Is  finite,  and  that  each  obstacle  consists  of  a  simple  polygon  together 
with  its  interior  (or  exterior). 

The  basic  idea  is  to  take  the  set  S  of  obstacles  and  construct  the 
associated  Voronoi  diagram  Vor(S)  (see  [2]).  This  diagram  is  a  planar 
network  of  straight  and  parabolic  arcs,  and  is  characterized  as  the  set 
of  points  which  are  equidistant  from  at  least  two  distinct  obstacles; 
it  can  be  computed  in  time  0(n  log  n)  by  an  algorithm  due  to 
Kirkpatrick  [2]  (a  somewhat  simpler  0(n  log~n)  algorithm  due  to  Lee  and 
Drysdale  [3]  is  also  available).  Let  B  be  a  circular  disc  of  radius  r. 
Our  technique  is  based  on  the  fact  (proved  below)  that  there  exists  a 
continuous  obstacle-avoiding  motion  it  of  the  disc  B  between  two 
specified  positions  Xq  ■  and  X|^  of  its  center  C  if  and  only  if  there 
exists  another  continuous  obstacle-avoiding  motion  it  '  of  B  from  Xq  to 
X|  during  which,  except'  for  its  initial  and  final  portions  (in  which  C 
is  moved  to  and  from  tfie  tToronoi  diagram),  the  center  G  moves  entirely 
along  the  Voronoi  diagram  Vor(S).  This  enables  us  to  reduce  the 
problem  to  searching  the  Voronoi  diagram. 


1 .  Motion-planning  for  a_  Disc. 

We  first  introduce  some  terminology.  'Distance'  will  always  mean 
Euclidean  distance  in  the  plane.  Let  S  be  the  set  of  obstacles,  let  r 
be  the  radius  of  the  moving  disc,  and  let  Xq  and  X]^  denote  the  two 
given  positions  of  its  center  C,  between  which  a  continuous 
obstacle-avoiding  motion  of  B  is  sought.  Write  2  for  the  set  of  points 
not  lying  in  any  obstacle.  An  instance  of  our  movers'  problem  is  a 
quadruple  <S,XQ,x2,r>,  where  S,xo,X]^  and  r  are  as  defined  above.  A 
solution  path  tt  to  <S,XQ,x^,r>  is  a  continuous  path  from  xq  to  xj^  such 
that  if  a  circle  B  of  radius  r  is  moved  with  its  center  along  tt  it  will 
not  collide  with  (or  touch)  any  of  the  objects  in  S. 
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Let  P  be  any  nonempty  set  of  points  In  the  plane.  Given  a  point 
X,  define  the  Hausdorff  distance  of  x  from  ?  as 

d(x,P)  =  inf  {d(x,p):  p  e    ?}. 
It  is  fairly  easy  to  show  that  for  some  p   in  the  closure  P  of  P, 
d(x,P)  =  d(x,p)  =  d(x,P).    It   is   also  well-known  that  the  Hausdorff 
distance  d(x,P)  is  continuous  in  x. 

For  the  time  being,  let  s  be  a  nonempty  compact  convex  set  in  the 
plane.  Again,  it  is  well-known  that  for  every  point  x  in  the  plane 
there  is  a  unique  point  Nearestg(x)  in  s  closest  to  x,  and  that  the  map 
NearestgC*  )  is  a  retraction  of  the  plane  onto  s.  (A  retraction  onto  s 
is  a  continuous  map  which  fixes  every  point  in  s.) 

Now  consider  a  set  of  obstacles  S  (which,  by  abuse  of  notation,  we 
will  identify  with  their  union).  Let  2  denote  the  (open)  complement  of 
S.  Clearly  the  Hausdorff  distance  from  x  to  S  is  attained  on  the 
boundary  of  S.  Let  Clearance(x)  denote  this  Hausdorff  distance.  As 
noted  above,  Clearance(«  )  is  continuous.  Note  that  the  boundary  of  S 
can  be  represented  as  a  finite  union  of  closed  line-segments:  thus 
Clearance(«  )  can  be  represented  as  the  minimum  of  a  finite  set  cjf 
continuous  Functions  which  are  easily  calculated. 

For  each  point  x  in  :^  define  Near(x)  to  be  the  set  of  points  y  in 
(the  boundary  of)  S  such  that  d(x,y)  =  Clearance(x) .  Since  S  is 
closed,  Near(x)  is  always  nonempty.  The  Voronoi  diagram  Vor(S)  of  S  is 
the  set  of  points  x  in  2  such  that  Near(x)  contains  more  than  one 
point. 

Lemma  1.  The  diagram  Vor(S)  is  closed  in  '?.  (though  not  in  the 
plane). 

Proof.  Let  x^  in  Vor(S)  converge  to  x  in  Si.  By  passing  to  a 
subsequence  if  necessary,  we  can  assume  that  for  all  n  there  exist 
points  Pj^  -t  q^  in  Near(xp,),  where  p^^  and  q,^  converge  to  p  and  q 
respectively.  We  can  further  assume  that  there  exist  closed 
wall-segments  s  *  s'  (i.e.,   open  wall-segments   together  with   their 
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endpoints)  such  Chat  pes  and  q  e  s'  for  all  n.  Since  s  and  s'  are 
closed,  pes  and  q  e  s',  and  by  continuity,  both  p  and  q  belong  to 
Near(x).  If  p  =  q  then  it  is  easy  to  see  that  this  point  is  a  conunon 
endpoint  of  s  and  s',  and  that  x  converges  to  this  point;  however,  it 
does  not  belong  to  J2 .   Hence  p  *  q  and  x  e  Vor(S).   Q.E.D. 

Define  the  mapping  Im  from  Q,  onto  Vor(S)  as  follows:  given  a  point 
X  in  ^ ,  choose  any  point  p  in  Near(x),  and  let  H  be  the  half-line  from 
p  through  X.  Then  Im(x)  is  the  point  closest  to  p  in  the  intersection 
of  H  with  Vor(S).  If  H  does  not  intersect  Vor(S)  then  Ira(x)  is 
undefined. 

Lemma  2.  If  Q  is  bounded  then  Im(x)  is  defined  uniquely  for  all  x 
in  n,  and  Im  is  a  retxaction  of  Q.  onto  Vor(S),  i.e.,  a  continuous  map 
onto  Vor(S)  which  fixes  every  point  in  Vor(S).  Moreover,  for  any  x  in 
n,  Clearance(«  )  is  strictly  increasing  along  the  line-segment  joining  x 
to  Im(x). 

Proof.   First  consider  any  point  x  in  2 ;  let  n  belong  to  Near(x). 
Claim   that   for   any   point  z   on  the  open  line-segment  joining  p  to  x, 
Near(z)  =  {p}.   This  is  because,  given  q  in  Near(z),  d(x,q)  >    d(x,o) 
d(x,z)  +  d(z,p)  >      d(x,z)  +  d(z,q);  therefore  xzq  is  a  straight  angle, 
so  p  =  q,  as  asserted. 

In  particular,  if  x  belongs  to  Vor(S)  then  Im(x)  =  x.  This 
resolves  the  apparent  ambiguity  when  Near(x)  is  not  a  singleton. 
Therefore,  to  show  that  Im  is  well-defined,  it  is  enough  to  show  that 
given  X  in  Q  \  Vor(S),  so  Near(x)  is  a  singleton  {p},  the  half-line  H 
from  p  through  x  intersects  Vor(S). 

Consider  the  points  p|^  and  P2,  defined  to  be  either  the  endpoints 
of  the  wall-segment  containing  p  (if  p  is  not  a  corner),  or  the  other 
endpoints  of  the  two  wall-segments  touching  p  (if  p  is  a  corner).  The 
angles  xppj^  and  xpp2  cannot  be  acute.  This  implies  that  there  exists  a 
neighborhood  U  of  p  (in  S,  not  in  the  plane)  such  that  U  and  x   lie   on 
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opposite  sides  of  the  line  through  p  perpeadicular  to  H.  Thus  p  is   the 
unique  point  in  U  nearest  to  every  point  in  H. 

Since  i^  is  bounded,  the  set  D  of  points  x'  in  H  such  that  Mear(x') 
*  {p}  is  nonempty.  Let  y  be  the  point  nearest  to  p  in  the  closure  of 
D.  Note  that  y  does  not  lie  in  the  open  line-segment  px.  By 
continuity,  d(y,p)  =  Clearance(y)  >  d(x,p),  so  y  lies  in  il .  Again  by 
continuity,  d(y,p)  =  d(y,S  \  U),  so  y  lies  in  Vor(S):  indeed,  y  = 
Im(x).  This  concludes  the  proof  that  Im  is  well-defined  and  fixes 
every  point  in  Vor(S). 

It  follows  from  the  above  argument  that  if  Near(x)  =  {p}  then 
Near(x')  =  {p}  for  every  point  x'  in  the  open  line-segment  joining  x  to 
Im(x):  thus  Clearance(')  is  strictly  increasing  along  this  segment,  and 
in  general,  Clearance(Im(x) )  >  Clearance(x) . 

'  ,    '".    :iB;'^ 

It  remains  to  show  continuity  of  Im.  If  x  in  Vor(S)  converges  to 
X  in  2 ,  then  by  Lemma  1  x  is  in  Vor(S),  so  Im(Xj^)  also  converges  to  x. 
Thus  we  need  only  consider  a  sequence  x  in  P.  \  Vor(S)  converging  to 
some  point  x  in  Q .  By  passing  to  a  subsequence  if  necessary,  we  can 
assume  that  NearCx^^)  =  {pp,}  and  Ira(xj^)  =  y^  where  the  sequences  p^^  and 
y^  are  cc:)nvergent ,  to  p  and  y  respectively.  3y  continuity, 
Clearance(y)  >  Clearance(x) ,  so  y  is  in  Q,  so  by  Lemma  1  y  is  in 
Vor(3).  Again  by  continuity,  x  lies  on  the  closed  line-segment  joining 
p  to  y,  and  d(p,y)  =  Clearance(y)  (so  p  e  Near(y)).  Therefore  Near(z) 
=  {p}  for  every  point  z  in  the  open  line-segment  joining  p  to  y.  It 
follows  immediately  that  y  =  Im(x).   Q.E.D. 

The  retraction  map  Im  is  simpler  than  might  appear  from  the  proof 
of  Lemma  2.  Given  that  the  boundary  of  S  consists  of  straight 
line-segments,  Vor(S)  is  a  planar  graph  whose  edges  are  composed  of 
finitely  many  straight  and  parabolic  line-segments:  the  wall-edges  and 
corners  are  respectively  the  directrices  and  foci  for  these  parabolic 
segments,  and  the  retraction  Im  moves  each  point  x  in  a  perpendicular 
direction  away  from  the  edge  nearest  x  or  in  a  radial  direction  outward 
from   the   corner   nearest  x.  (Note  that  the  Voronoi  diagram  as  defined 
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here  is  a  subgraph  of  the  generali^ed  Voronoi  diagram  defined  in  [2], 
which  includes  straight  edges  perpendicular  to  the  open  wall-segments 
through  their  endpoints.) 


We  can  now  state  and  prove  the  main  result  of  this  paper: 


Theorem.   Suppose  that  Q    is  bounded,  and  Xq  and  Xi  are  free   placements 

for   the   disc  B  in  S.  Then  <S,XQ,X]^,r>  admits  a  solution  path  it  if  and 

only  if  <S,  Im(xQ)  ,Im(x^)  ,r>  admits   a   solution   path  tt  '   which   lies 
entirely  in  Vor(S).      •■ 

Proof.  Note  that  since  Q  is  bounded,  the  map  Im  is  well-defined 
and  continuous. 

First  suppose  that  <S,XQ,Xi,r>  admits  a  solution  path  n .  Let  tt  '  = 
Im(TT).  Clearly,  ir  '  lies  entirely  in  Vor(S)  and  joins  Im(xg)  to  Im(xj^). 
Furthermore,  by  Lemma  2,  Clearance(lm(x' ) )  >  Clearaace(x' )  >  r  for 
every  point  x'  in  tt  ,  so  tt  '  is  a  solution  path. 

Conversely,  let  tt  '  be  a  solution  path  joining  Ira(xQ)  to  ImCx^  and 
lying  entirely  in  Vor(S).  Define  tt  as  follows:  first  move  (the  center 
C  of)  B  directly  from  Xq  to  Im(xQ),  then  move  C  along  it',  and  finally 
move  C  directly  from  Im(xp  to  Xp  Since  Xq  and  xj^  are  free 
placements,  it  again  follows  from  Lerana  2  that  every  placement  between 
Xq  and  ImCxg)  and  between  Im(xj^)  and  Xj^  is  free,  so  n  is  a  solution 
path  for  <S,xq,Xj^  ,r>.   Q.E.D. 

Our  method  will  not  work,  if  the  space  FP  of  free  placements  is  not 
bounded.  'Por  example,  in  Figure  1,  there  is  a  motion  from  Xg  to  Xj^, 
but  it  must  leave  the  Voronoi  diagram.  This  problem  can  be  fixed  by 
'framing'  the  region  in  which  the  disc  is  allowed  to  move;  within  this 
frame,  all  Voronoi  cells  are  bounded,  and  the  problem  disappears. 
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Figure  1:  where  Che  disc  must  leave  Che  Voronot  diagram. 


To  ensure  ChaC  FP  is  bounded,  choose  a  large  recCangle  M  and 
define  a  further  obstacle  F  (a  frame)  consisting  of  all  points  on  or 
outside  M.  Choose  M  large  enough  so  that  all  the  obstacles  in  S  (of 
which  there  are  only  finitely  many),  and  also  Xq  and  x^ ,  are  at  a 
distance  greater  than  2r  from  F.  Let  the  new  set  S'  of  obstacles  be  the 
union  of  S  with  {F}.  We  claim  that  there  exists  a  solution  path  for 
<S,XQ,xi,r>  if  and  only  if  there  exists  a  solution  path  for 
<3' ,xg,x^ ,r>.  To  see  this,  note  first  that  anv  solution  path  for 
<S' ,Xq,X]^  ,r>  is  also  a  solution  path  for  <3,xo,x^,r>.  Conversely,  any 
solution  path  tt  for  <S,XQ,xj^,r>  gives  rise  to  a  path  tt  '  for 
<S' ,xq,xj^  ,r>,  as  follows:  Let  M'  be  the  set  of  points  inside  M  which 
are  at  distance  (3r/2)  from  F.  Note  that  M'  is  itself  a  closed 
rectangle.  Then  tt  '  is  obtained  by  'truncating'  those  portions  of  ir 
which  lie  outside  M'  and  replacing  them  with  appropriate  segments  of 
M'. 


Remark.  The  above  framing  transformation  is  apparently 
parameterized  by  r,  but  it  can  be  handled  in  a  symbolic  manner 
independent  of  the  value  of  r.  Alternatively,  we  can  work  in  the 
projective  plane  and  then  lm(p)  is  always  defined  (possibly  as  a  point 
at  infinity).  This  observation  will  be  pertinent  to  the  discussion  in 
section  3. 
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The  preceding  discussion  justifies  the  following  algorithm: 


1.  (Preprocessing)  If  necessary,  modify  S  by  framing  it  along  the 
lines  described  above.  Decompose  the  boundary  of  S  into  a  set 
W  of  corners  and  open  wall-segments.  Let  n  be  the  number  of 
objects  in  W. 

2.  Construct  the  Voronoi  diagram  Vor(S),  using,  for  instance, 
Kirkpatrick's  0(n  log  n)  algorithm  [2].  The  diagram  Vor(S)  is 
a  union  of  0(n)  straight  and  parabolic  arcs.  Kirkpatrick's 
algorithm  can  specify  it  as  a  (combinatorial)  graph  N,  in 
which  each  edge  is  labelled  with  an  algebraic  equation 
defining  it  as  a  curve  in  the  plane,  and  each  vertex  is 
labelled  with  its  co-ordinates.  Finally,  from  the  equation 
defining  an  edge  e  and  the  co-ordinates  of  its  endpoints ,  one 
can  compute  Its  width  -  the  minimum  clearance  along  e;  this 
information  can  also  be  linked  to  e.  The  overall  cost  of  this 
extra  processing  is  0(n). 

3.  (Motion  planning)  Given  two  points  Xq  and  x^  in  the  plane,  it 
is  required  to  move  the  center  of  the  disc  B  from  .'-Cq  to  xj^ 
while  not  touching  any  obstacle.  By  considering  in  turn  every 
object  in  W,  one  can  compute  the  set  Near(xQ)  and  the  quantity 
Clearance(xQ).  At  this  point,  if  Clearance(xo)  «  r,  the 
initial  placement  is  not  free:  stop  with  an  error  message.  If 
not,  one  can  then  compute  Im(xQ),  as  follows.  If  Near(xo) 
contains  more  than  one  point,  then  Im(xQ)  =  xq.  Otherwise, 
say  Near(xQ)  =  {p}.  Again  by  considering  all  objects  in  W, 
one  can  locate  the  first  point  Vq  on  the  semi-infinite  line  H 
from  p  through  Xq  such  that  yg  is  equidistant  from  p  and  some 
other  point  in  S.  Then  yg  =  Im(xo).  By  searching  the  graph  N 
one  can  determine  the  edge  Bq  containing  vq.  Similarly,  one 
can  determine  the  edge  e^^  of  N  containing  y|  =  Im(x]^),  and 
stop  if  Xj^  is  not  free.   All  this  can  be  done  in  time  0(n). 

4.  If  eQ  =  ej^,  then  one  can  determine  explicitly  whether  the 
clearance  exceeds  r  between  yg  and  y^  along  this  edge.  This 
takes  constant  time. 


-10- 

5.  If  ^0  "*  ^1>  then  one  can  determine  which,  if  any,  of  the 
endpoints  of  Bq  can  be  reached  from  vq  while  maintaining 
sufficient  clearance;  similarly  for  Vi.  Assuming  that  an 
endpoint  Zq  of  eg  and  z^  of  e^  can  be  so  reached,  the 
motion-planning  problem  now  reduces  to  finding  a  path  from  Zq 
to  Zj^  in  N  along  edges  of  width  greater  than  r.  This  can  be 
done  in  time  0(n). 


3.  Final  remarks 


It  is  instructive  to  compare  our  Voronoi  diagram  approach  to  that 
used  in  previous  papers  [7,8,9]  which  can  be  called  the  critical  region 
approach.   (We  discuss  only  the  case  of  a  disc.)       ^, 

^-Then  applied  to  a  single  moving  disc,  the  critical  region  approach 
also  gives  rise  to  an  0(n  log  n)  algorithm.  Indeed,  this  particularly 
simple  path  problem  reduces  to  computing  the  "r-fringe"  of  the  given 
set  of  obstacles  S,  where  the  r-fringe  consists  of  those  points  in  \i 
which  are  at  distance  r  from  tlie  nearest  obstacle.  It  is  easy  to  give 
an  0(n)  algorithm  for  computing  the  r-fringe  given  the  Voronoi  diagram. 
Once  this  'fringe'  is  available,  the  connected  components  of  its 
complement  are  easily  obtained.  (However,  we  do  not  know  how  to  obtain 
the  r-fringe  in  0(n  log  n)  time  without  using  an  0(n  log  n)  algorithm 
to  compute  the  Voronoi  diagram). 

In  practical  terms,  the  solution  path  obtained  by  the  Voronoi 
diagram  approach  should  be  a  particularly  reliable  way  to  avoid 
collisions,  since  it  keeps  the  disc  as  far  as  possible  from  the 
obstacles.  Again,  note  that  for  both  approaches  the  preprocessing  time 
is  0(n  log  n)  and  the  path-finding  time  is  0(n).  However,  in  the 
critical  region  approach  the  data-structure  established  during 
preprocessing  can  only  be  used  to  plan  motions  for  a  disc  of  fixed 
size;    a   significant    advantage   of   our   approach   is   that   the 


-11- 

data-strucCure  established  in  Che  preprocessing  phase  can   be   used   to 
solve  motion-planning  problems  for  discs  of  any  size. 
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